#include "msg_latest_storage_strategy.h"

#include "../utils/object.h"

#include <iostream>
namespace fusion_perception {

MsgLatestStorageStrategy::MsgLatestStorageStrategy(int64_t microThreshold)
    : microThreshold_(microThreshold)
{
}

void MsgLatestStorageStrategy::push(ObjectSptrVector msg)
{
    std::lock_guard<std::mutex> locker(mutexLock_);

    msg_ = msg;
}

ObjectSptrVector MsgLatestStorageStrategy::take(int64_t microTs)
{
    if (msg_.empty()) {
        return ObjectSptrVector();
    }

    std::lock_guard<std::mutex> locker(mutexLock_);
    auto dt = microTs - (int64_t(msg_.at(0)->header.stamp.sec) * 1000000 + msg_.at(0)->header.stamp.nanosec / 1000);

    if (msg_.size() != 0 && dt < microThreshold_) {
        return msg_;
    }

    return ObjectSptrVector();
}
}